#include "Algebre/TransformationHomogene.hpp"

#include <iostream>

using namespace std;

void printTransform( const TransformationHomogene& trans )
{
    for ( size_t i( 0u ), j; i < 4u; ++i )
    {
        for ( j = 0u; j < 4u; ++j )
            cout << trans( i, j ) << ' ';
        cout << endl;
    }
}

int main()
{
    VecteurU x( 1., 0., 0. );
    VecteurU y( 0., 1., -1. );
    VecteurU z( 0., 1., 1. );

    MatriceSO3 rot( x, y, z );

    TransformationHomogene hom1( rot );

    cout << "Rotation:" << endl;
    printTransform( hom1 );

    Vecteur trans( 40., 0., -10. );

    TransformationHomogene hom2( trans );

    cout << "Translation:" << endl;
    printTransform( hom2 );

    TransformationHomogene comp;

    comp = hom2 * hom1;

    cout << "Composition:" << endl;
    printTransform( comp );

    TransformationHomogene inv;

    if ( ! comp.inverse( inv ) )
        cerr << "Impossible d'inverser la transformation" << endl;
    else
    {
        cout << "Inverse:" << endl;
        printTransform( inv );
    }

    return 0;
}
